
clear
close all
%datasets raw.dat and nav.dat are made from corresponding Raw* and Nav* files 
%by stripping the headers and removing imcoplete last lines
load ~/jsvn/GIM/src/staticdata/datasets/Avant635/19_04_2011/secondDataset/raw.dat
load ~/jsvn/GIM/src/staticdata/datasets/Avant635/19_04_2011/secondDataset/nav.dat
load ~/jsvn/GIM/src/staticdata/datasets/Avant635/19_04_2011/secondDataset/mpos.dat
load ~/jsvn/GIM/src/staticdata/datasets/Avant635/19_04_2011/secondDataset/6D_Laser_file.txt
load ~/jsvn/GIM/src/staticdata/datasets/Avant635/19_04_2011/secondDataset/6D_Odo_file.txt
mpos=mpos(2:end,:);
laser=X6D_Laser_file(:,[6:9 3:5]);
timl=X6D_Laser_file(:,1)+X6D_Laser_file(:,2)/1e6;
odo=X6D_Odo_file(:,[6:9 3:5]);
timo=X6D_Odo_file(:,1)+X6D_Odo_file(:,2)/1e6;
timm=mpos(:,1)+mpos(:,2)/1e6;

for i=1:length(mpos(:,1))
theta=mpos(i,8);
n=[0 0 -1];
mapo(i,1:4)=[cos(theta/2) n*sin(theta/2)];
R=quater2rot(mapo(i,1:4));
Rt=R*mpos(i,3:5)';
mapo(i,5:7)=Rt';
end

for i=1:length(odo(:,1))
R=quater2rot(odo(i,1:4));
Rt=R*odo(i,5:7)';
odo(i,5:7)=Rt';
end
for i=1:length(laser(:,1))
R=quater2rot(laser(i,1:4));
Rt=R*laser(i,5:7)';
laser(i,5:7)=Rt';
end



acc=raw(:,11:13);
gyro=raw(:,8:10);
%estimate bias from first 1000 measurements
gb=mean(gyro(1:1000,:))
gyroc=gyro-repmat(gb,length(gyro(:,1)),1);
tim=raw(:,1)+raw(:,2)/1e6;
timn=nav(:,1)+nav(:,2)/1e6;
%load files created with voslam read voslam/readme.txt
load ../voslam/pose.dat
pose1=pose;
for i=1:length(pose(:,1))
R=quater2rot(pose(i,1:4));
Rt=R*pose(i,5:7)';
pose(i,5:7)=Rt';
end
load ../voslam/timestamp.dat
%one needs to use previous images timestamp: 
tims=[0 1303216062 080537]; timestamp=[tims;timestamp];
timc=timestamp(:,2)+timestamp(:,3)/1e6;
dt=tim(2:end)-tim(1:end-1);
tic
for k=1:500

  for i=1:length(timc)-1
    of=0.25-k/1000;
    [m h]=min(abs(timc(i)-tim((timc(i)-tim+of)>0)+of));
    indt(i)=h;
    me(i)=m;
  end

  for i=1:length(timc)-1
    [m h]=min(abs(timc(i)-timl+0.25-k/1000));
    indl(i)=h;
    mel(i)=m;
  end
  for i=1:length(timc)-1
    [m h]=min(abs(timc(i)-timo+0.25-k/1000));
    indo(i)=h;
    meo(i)=m;
  end


  for i=1:length(timc)-3
%need to multiply with 1.1
    start=indt(i)+1;
    stop=indt(i+1)-1;
    gyr=gyroc(start:stop,:).*repmat(dt(start:stop),1,3);
    qyr=1.15*[gyroc(indt(i),:)*me(i);gyr;gyroc(indt(i+1),:)*(dt(indt(i+1))-me(i+1))];
    q0=[1 0 0 0];
    q=integrate_gyros(qyr',q0);
    qg(:,i)=q(:,end);
    phig(i)=acos(qg(1,i))*360/pi;
  end

  for i=1:length(pose(:,1))-1
    rpose(i,:)=relative_transf(pose(i+1,1:7),pose(i,1:7));
    rpose(i,1:4)=rpose(i,1:4)/norm(rpose(i,1:4));
    if(rpose(i,1)<0)
      rpose(i,1:4)=-rpose(i,1:4);
    end
    phi(i)=acos(rpose(i,1))*360/pi;
  end
  for i=1:length(timc)-3
    lpose(i,:)=relative_transf(laser(indl(i+1),1:7),laser(indl(i),1:7));
    lpose(i,1:4)=lpose(i,1:4)/norm(lpose(i,1:4));
    if(lpose(i,1)<0)
      lpose(i,1:4)=-lpose(i,1:4);
    end
    phil(i)=acos(lpose(i,1))*360/pi;
  end
  
  for i=1:length(timc)-3
    opose(i,:)=relative_transf(odo(indo(i+1),1:7),odo(indo(i),1:7));
    opose(i,1:4)=opose(i,1:4)/norm(opose(i,1:4));
    
    if(opose(i,1)<0)
      opose(i,1:4)=-opose(i,1:4);
    end
    phio(i)=acos(opose(i,1))*360/pi;
  end

  er=phi-phig(1:end);
  sume(k)=er*er';
  er=phi-phil(1:end);
  sumel(k)=er*er';
  er=phi-phio(1:end);
  sumeo(k)=er*er';
end
toc
plot([sume' sumel' sumeo' ])
[m h]=min(sume)
[m h]=min(sumel)
[m h]=min(sumeo)

for i=1:length(timc)-1
    of=0.25-215/1000;    
    [m h]=min(abs(timc(i)-tim((timc(i)-tim+of)>0)+of));
    indt1(i)=h;
    me1(i)=m;
end
for i=1:length(timc)-1
    [m h]=min(abs(timc(i)-tim+0.25-215/1000));
    indt(i)=h;
    me(i)=m;
end
for i=1:length(timc)-1
    [m h]=min(abs(timc(i)-timl+0.25-57/1000));
    indl(i)=h;
    mel(i)=m;
end
for i=1:length(timc)-1
    [m h]=min(abs(timc(i)-timo+0.25-149/1000));
    indo(i)=h;
    meo(i)=m;
end

for i=1:length(timc)-3
%need to multiply with 1.1
    start=indt1(i)+1;
    stop=indt1(i+1)-1;
    gyr2=1.16*gyroc(start:stop,:).*repmat(dt(start:stop),1,3);
    gyr3=[gyroc(indt1(i),:)*me1(i);gyr2;gyroc(indt1(i+1),:)*(dt(indt1(i+1))-me1(i+1))];
    q0=[1 0 0 0];
    q=integrate_gyros(gyr3',q0);
    qg(:,i)=q(:,end);
    phig(i)=acos(qg(1,i))*360/pi;
end

for i=1:length(timc)-3
%need to multiply with 1.1
    start=indt1(i)+1;
    stop=indt1(i+1)-1;
    gyr1=gyroc(start:stop,:).*repmat(dt(start:stop),1,3);
%    qyr1=[gyroc(indt1(i),:)*me1(i);gyr1;gyroc(indt1(i+1),:)*(dt(indt1(i+1))-me1(i+1))];
    q0=[1 0 0 0];
    q=integrate_gyros(gyr1',q0);
    qg2(:,i)=q(:,end);
    phig2(i)=acos(qg2(1,i))*360/pi;
end

  
for i=1:length(timc)-3
%need to multiply with 1.1
    gyr=gyroc(indt(i):indt(i+1),:).*repmat(dt(indt(i):indt(i+1)),1,3);
    q0=[1 0 0 0];
    q=integrate_gyros(gyr',q0);
    qg(:,i)=q(:,end);
    phig1(i)=acos(qg(1,i))*360/pi;
end

for i=1:length(pose(:,1))-1
    rpose(i,:)=relative_transf(pose(i+1,1:7),pose(i,1:7));
    rpose(i,1:4)=rpose(i,1:4)/norm(rpose(i,1:4));
    if(rpose(i,1)<0)
      rpose(i,1:4)=-rpose(i,1:4);
    end
    phi(i)=acos(rpose(i,1))*360/pi;
end
for i=1:length(timc)-3
    lpose(i,:)=relative_transf(laser(indl(i+1),1:7),laser(indl(i),1:7));
    lpose(i,1:4)=lpose(i,1:4)/norm(lpose(i,1:4));
    lgpose(i,:)=laser(indl(i),1:7);
    R=quater2rot(lgpose(i,1:4));
    Rt=R'*lgpose(i,5:7)';
    lgpose(i,5:7)=Rt';
    if(lpose(i,1)<0)
      lpose(i,1:4)=-lpose(i,1:4);
    end
    phil(i)=acos(lpose(i,1))*360/pi;
end
lgpose(i,:)=laser(indl(i),1:7);
R=quater2rot(lgpose(i,1:4));
Rt=R'*lgpose(i,5:7)';
lgpose(i,5:7)=Rt';

for i=1:length(timc)-3
    mpose(i,:)=relative_transf(mapo(indl(i+1),1:7),mapo(indl(i),1:7));
    mpose(i,1:4)=mpose(i,1:4)/norm(mpose(i,1:4));
    mgpose(i,:)=mapo(indl(i),1:7);
    R=quater2rot(mgpose(i,1:4));
    Rt=R'*mgpose(i,5:7)';
    mgpose(i,5:7)=Rt';
    if(mpose(i,1)<0)
      mpose(i,1:4)=-mpose(i,1:4);
    end
    phim(i)=acos(mpose(i,1))*360/pi;
end
mgpose(i,:)=mapo(indl(i),1:7);
R=quater2rot(mgpose(i,1:4));
Rt=R'*mgpose(i,5:7)';
mgpose(i,5:7)=Rt';
  
for i=1:length(timc)-3
    opose(i,:)=relative_transf(odo(indo(i+1),1:7),odo(indo(i),1:7));
    opose(i,1:4)=opose(i,1:4)/norm(opose(i,1:4));
    ogpose(i,:)=odo(indo(i),1:7);
    R=quater2rot(ogpose(i,1:4));
    Rt=R'*ogpose(i,5:7)';
    ogpose(i,5:7)=Rt';
    if(opose(i,1)<0)
      opose(i,1:4)=-opose(i,1:4);
    end
    phio(i)=acos(opose(i,1))*360/pi;
end
ogpose(i,:)=odo(indo(i),1:7);
R=quater2rot(ogpose(i,1:4));
Rt=R'*ogpose(i,5:7)';
ogpose(i,5:7)=Rt';


erg=phi-phig(1:end);
erl=phi-phil(1:end);
ero=phi-phio(1:end);
plot([phig' phil' phio' phi' phim'])

save 


for i=1:length(pose1(:,1))
t(i,:)=pose1(i,5:7)*Ro8;
end
plot(t(:,1),t(:,3))
axis equal
plot(t(:,2),t(:,3))
axis equal

plot(mgpose(1:40,5),mgpose(1:40,6),'*',lgpose(1:40,5),lgpose(1:40,6),'r*',t(1:40,1),t(1:40,2),'g*')

[u,d,v]=svd(rpose(:,5:7),0)
v(:,3)'*rpose(:,5:7)'*rpose(:,5:7)*v(:,3)

[u,d,v]=svd(pose1(:,5:7),0);
v(:,3)'*pose1(:,5:7)'*pose1(:,5:7)*v(:,3)

n=v(:,3)
norm1=sqrt(n(1)*n(1)+n(2)*n(2)+n(3)*n(3));    
theta=acos(n(3)/norm1);
oq(1)=cos(theta/2);
om(1)=n(2);
om(2)=-n(1);
om(3)=0;
norm1=sqrt(om(1)*om(1)+om(2)*om(2));
if(norm1>2*1e-16)

        oq(2)=sin(theta/2)*om(1)/(norm1);
        oq(3)=sin(theta/2)*om(2)/(norm1);
        oq(4)=0;

end
Ro1=quater2rot(oq)

spos=rpose((rpose(:,1))>0.9999,5:7);
[u,d,v]=svd(spos,0);
v(:,1)'*spos'*spos*v(:,1)
dir=v(:,1)

qpos=rpose((rpose(:,1))<0.999,2:4);
[u,d,v]=svd(qpos,0);
v(:,1)'*qpos'*qpos*v(:,1)
qdir=v(:,1)
 
dir'*qdir

ns=sqrt(sum(spos.^2,2));
nq=sqrt(sum(qpos.^2,2));
spos1=spos./repmat(ns,1,3);
qpos1=qpos./repmat(nq,1,3);

[u,d,v]=svd(spos1,0);
v(:,1)'*spos1'*spos1*v(:,1)
dir1=v(:,1)

[u,d,v]=svd(qpos1,0);
v(:,1)'*qpos1'*qpos1*v(:,1)
qdir1=v(:,1)
 
dir1'*qdir1

u=dir1
v=qdir1
u2=v-v'*u*u

load ../calibration/Ro.mat

[qg(1:4,1:40)' rpose(1:40,1:4)]

qpos=rpose((rpose(:,1))<0.999,2:4);
qq=qg(2:4,(rpose(:,1))<0.999)';
ergg=erg((rpose(:,1))<0.999)';
phg=acos(rpose(rpose(:,1)<0.999,1))*360/pi
qpos=qpos(:,abs(ergg./phg)<0.05)
qq=qq(:,abs(ergg./phg)<0.05)
nc=sqrt(sum(qq.^2,2));
nq=sqrt(sum(qpos.^2,2));
qq1=qq./repmat(nc,1,3);
qpos1=qpos./repmat(nq,1,3);
[u,d,v]=svd(qq'*qpos)
Ro2=u*v'
 
[u,d,v]=svd(qq1'*qpos1)
Ro3=u*v'

[u,d,v]=svd(qq1,0);
v(:,1)'*qq1'*qq1*v(:,1)
qdi1=v(:,1)

q2=rot2quater(Ro2)
q8=rot2quater(Ro8)

for i=1:length(pose1(:,1))
Rt=Ro8'*pose1(i,5:7)';
t(i,:)=Rt';
end
